/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.templates.Recoverable;

/**
 *
 * @author matt
 */
public class ElectricSwerveSubsystem extends SwerveSubsystem implements Recoverable {
    // Put methods for controlling this subsystem
    // here. Call these from Commands.

    private CANJaguar m_swerveDrive = null;
    private int m_swerveMotorCANid = 0;
    // Try up to 5 times
    private static final int m_kMaxRetries = 5;
    private static double raisedPosition = 0.307;
    private static double loweredPosition = 0.817;
    private double position = 0;
    private static double iteritivePosition = 0.5;

    public ElectricSwerveSubsystem(int motorCANID) {
        System.out.println("Constructing ElectricSwerveSubsystem");

        m_swerveMotorCANid = motorCANID;

        int tries = 0;
        do {
            try {
                m_swerveDrive = new CANJaguar(m_swerveMotorCANid);
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ElectricSwerveSubsystem(): CAN Timeout while creating motor interface for CAN ID " + m_swerveMotorCANid);
                // set this motor to null since it wouldn't create (that way other parts of the class can tell not to use it)
                m_swerveDrive = null;
            } catch (Exception all) {
                System.out.println(all.toString());
            }
            tries++;
        } while ((null == m_swerveDrive) && tries < m_kMaxRetries);
        if ((null == m_swerveDrive)) {
            System.out.println("ElectricSwerveSubsystem(): created motor " + m_swerveMotorCANid);
        }
        System.out.println("Motor Created  - " + m_swerveMotorCANid);
        initialize();
    }

    private void initialize() {

        if (null != m_swerveDrive) {
            double gain_Kp = 750.0; // Proportional
            double gain_Ki = 1; // Integral
            double gain_Kd = 0.0; // Derivative

            int tries = 0;
            boolean configured = false;
            do {
                try {
                    m_swerveDrive.changeControlMode(CANJaguar.ControlMode.kPosition);
                    m_swerveDrive.setPositionReference(CANJaguar.PositionReference.kPotentiometer);
                    m_swerveDrive.setPID(gain_Kp, gain_Ki, gain_Kd);
                    // Ratios on Swerve Proxy 5*12.75*(14/8)^3 = 341
                    //m_swerveDrive.configPotentiometerTurns(345);  // 1 turn pot
                    m_swerveDrive.configPotentiometerTurns(1);  // 1 turn pot
                    //m_swerveDrive.configSoftPositionLimits(0, 25);
                    m_swerveDrive.enableControl(0);
                    configured = true;
                } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                    System.out.println("ElectricSwerveSubsystem(): CAN Timeout while configuring swerve CAN ID " + m_swerveMotorCANid);
                } catch (Exception all) {
                    System.out.println(all.toString());
                }
                tries++;
            } while (!configured && (tries < m_kMaxRetries));
            if (configured) {
                System.out.println("ElectricSwerveSubsystem(): Configured swerve motor " + m_swerveMotorCANid);
            }
        }
        System.out.println("ElectricSwerve Created with motor - " + m_swerveMotorCANid);
    }

    public void raiseSwerve() {
        initialize();
//        iteritivePosition += .1;
//        raisedPosition = iteritivePosition;
        System.out.println("ElectricSwerveSubsystem: Raise Swerve " + raisedPosition);
        if (null != m_swerveDrive) {
            try {
                System.out.println("Raising to " + raisedPosition);
                m_swerveDrive.setX(raisedPosition);
                m_kSwerveStateDeployed = false;
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ElectricSwerveSubsystem.raiseSwerve(): CAN Timeout while raising swerve motor " + m_swerveMotorCANid);
            }

        }
    }

    public void lowerSwerve() {
        initialize();
        //       iteritivePosition -= .1;
        //       loweredPosition = iteritivePosition;
        System.out.println("ElectricSwerveSubsystem: Lower Swerve " + loweredPosition);
        if (null != m_swerveDrive) {
            try {
                System.out.println("Lowering to " + loweredPosition);
                m_swerveDrive.setX(loweredPosition);
                m_kSwerveStateDeployed = true;
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ElectricSwerveSubsystem.raiseSwerve(): CAN Timeout while raising swerve motor " + m_swerveMotorCANid);
            }
        }
    }

    public void setPosition(double position) {
        initialize();
        System.out.println("ElectricSwerveSubsystem: setPosition " + loweredPosition);
        if (null != m_swerveDrive) {
            try {
                System.out.println("Lowering to " + position);
                m_swerveDrive.setX(position);
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ElectricSwerveSubsystem.setPosition(): CAN Timeout while raising swerve motor " + m_swerveMotorCANid);
            }
        }
    }

    public double getPosition() {
        position = 0;
        if (null != m_swerveDrive) {
            try {
                position = m_swerveDrive.getPosition();
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ElectricSwerveSubsystem.getPosition(): CAN Timeout getting postion on motor " + m_swerveMotorCANid);
            }
            //System.out.println("ElectricSwerveSubsystem.getPosition() = " + position);
        }
        return position;
    }

    public double getCommandedPosition() {
        position = 0;
        if (null != m_swerveDrive) {
            try {
                position = m_swerveDrive.getX();
            } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
                System.out.println("ElectricSwerveSubsystem.getPosition(): CAN Timeout getting postion on motor " + m_swerveMotorCANid);
            }
            //System.out.println("ElectricSwerveSubsystem.getPosition() = " + position);
        }
        return position;
    }

    /*
     * Power drop recovery support
     */
    public void recover(boolean override) {
        boolean needsRecovery = false;

        // figure out if the Jaguar has rebooted
        try {
            if (null != m_swerveDrive) {
                needsRecovery = m_swerveDrive.getPowerCycled();
            }
        } catch (edu.wpi.first.wpilibj.can.CANTimeoutException e) {
            System.out.println("ElectricSwerveSubsystem(): CAN Timeout while configuring swerve CAN ID " + m_swerveMotorCANid);
        }

        // and if it did, reinitialize
        if (needsRecovery || override) {
            initialize();
        }
    }
}
